#!/usr/bin/python3

import rospy
from sensor_msgs.msg import Image
from block_detection.srv import *
from cv_bridge import CvBridge
import cv2 as cv
import numpy as np


class Detection:
    def __init__(self):
        # 角点值
        self.corner_point = np.float32([[40, 33], [603, 30], [609, 454], [38, 454]])
        # hsv通道
        self.hsv_color = [[42, 34, 28], [142, 192, 142]]
        # 计算变换矩阵
        pst_obs = np.float32([[0, 0], [400, 0], [400, 300], [0, 300]])
        self.Mat = cv.getPerspectiveTransform(self.corner_point, pst_obs)   

    def ros_running(self):
        rospy.init_node("block_detection")
        camera_name = rospy.get_param("~camera_name", '')
        rospy.Subscriber(camera_name,Image,self.image_callback)
        rospy.Service('block_detection',block_result,self.detection_callback)
        rospy.spin()
        cv.destroyAllWindows()


    def image_callback(self, frame):
        bridge = CvBridge()
        # 消息转换为rgb uint8
        self.frame = bridge.imgmsg_to_cv2(frame, 'bgr8')
        rec = self.block_detection(self.frame)
        print(rec)
        cv.imshow("image",self.mask)
        cv.waitKey(22)



    def detection_callback(self, req):
        if req:
            return block_resultResponse(self.block_detection(self.frame),True)
        else:
            return block_resultResponse([],False)

    
    def block_detection(self, image):
        # 透视变换
        roi = cv.warpPerspective(image, self.Mat, (400, 300))
        # 颜色提取
        lower_color = np.uint8(self.hsv_color[0])
        upper_color = np.uint8(self.hsv_color[1])
        hsv = cv.cvtColor(roi, cv.COLOR_BGR2HSV)
        mask = cv.inRange(hsv, lower_color, upper_color)
        self.mask = mask
        origin = [0, 0] # 原点
        length = 80 # box长度
        dst_x = 107 # 偏移值
        dst_y = 72
        result = []
        for i in range(4):
            for j in range(4):
                spilt1 = mask[origin[1] + dst_y * i:origin[1] + length + dst_y * i,
                         origin[0] + dst_x * j:origin[0] + length + dst_x * j]
                res_sum = sum(spilt1.ravel() > 0)
                if res_sum > 800:
                    num = 1
                else:
                    num = 0
                result.append(num)
        return result


if __name__ == "__main__":
    det = Detection()
    det.ros_running()
        